package edu.csusm.spotrobot;

import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.squawk.util.StringTokenizer;
import java.io.IOException;

/**
 * Communicates with the PIC to receive odometry (how far each wheel travelled) 
 * and speed.
 *
 * @author Zebulon Evans
 */
public class OdometrySensor {
    
    private EDemoBoard demo = EDemoBoard.getInstance();
    
    /**
     * Gets the ticks for the left and right wheels to see how far they have 
     * travelled.
     *
     * @return array of ints the first is the left ticks and second is right ticks.
     */
    public int[] getTicks(){
        // ask the PIC for the odometry string and parse it.
        flushBuffer();

        // send get ticks command and print out ack and possibly the command complete
        demo.writeUART((byte)0x21);
        demo.writeUART((byte)84);
        flushBuffer();
        demo.writeUART((byte)0xEE);
        byte[] odoBytes = new byte[200];
        char[] odoChars = new char[200];
        String leftTicks = "0";
        String rightTicks = "0";    
        System.out.println("Requesting getTicks command 84");
        try{
            Thread.sleep(300);
            int numBytesRead = demo.availableUART();
            demo.readUART( odoBytes, 0, numBytesRead);
            for( int i=0; i < numBytesRead; i++){
                odoChars[i] = (char)odoBytes[i];
            }
            StringBuffer strBuff = new StringBuffer( String.valueOf(odoChars, 0, numBytesRead) );
            // trim off the ack
            strBuff = strBuff.delete( 0, 3 );
            // trim off the complete
            strBuff = strBuff.delete( numBytesRead - 6,numBytesRead - 1);
            
            StringTokenizer strTokenizer = new StringTokenizer( strBuff.toString(), "," );
            
            leftTicks = strTokenizer.nextToken();

            rightTicks = strTokenizer.nextToken();

            System.out.println("Received getTicks parsed: " + leftTicks + " " + rightTicks );
            
        }catch(Exception e){} 
        
        int[] tickArray = new int[2];
        tickArray[0] = Integer.parseInt(leftTicks);
        tickArray[1] = Integer.parseInt(rightTicks);
        return tickArray;
        
    }
    
    /**
     * Gets the ticks per second that the wheels are moving from the PIC.
     *
     * @return int representing the ticks per second
     */
    public int getTicksPerSecond(){
        flushBuffer();

        // send get ticks command and print out ack and possibly the command complete
        demo.writeUART((byte)0x21);
        demo.writeUART((byte)0x85);
        flushBuffer();
        demo.writeUART((byte)0xEE);
        byte[] odoString = new byte[200];
        System.out.println("Requesting getTicksPerSecond command 85");
        try{
            Thread.sleep(300);
            demo.readUART( odoString, 0, 100);
            System.out.println("Received getTicksPerSecond ack: " + odoString);
        }catch(Exception e){} 
        
        return 0;
    }
    
    /**
     * Flush the recieve buffer for the UART for serial communications.
     */
    private void flushBuffer(){
        // TODO: make a static class that has this type of stuff in it
        //flush receive buffer.
         try
         {
            while(demo.availableUART() > 0)
               demo.readUART(100);
         }
         catch(IOException ioe) {} 
    }

}
